/*
 * Conversion.cpp
 *
 *  Created on: Mar 23, 2013
 *      Author: ohnozzy
 */

#include "Conversion.h"

void flyingcamera::conversion::moveUpandTranslate(fcl::Transform3f& tf, double cone_length, double x, double y ){
    	    const fcl::Matrix3f& m=tf.getRotation();
    	    double half_length=cone_length/2;
    	    fcl::Vec3f trans(half_length*m(0,2)+x,half_length*m(1,2)+y,half_length*m(2,2));
            tf.setTranslation(trans);


}
void flyingcamera::conversion::ompl2fclRotation(const ompl::base::SO3StateSpace::StateType& ompl_r, fcl::Quaternion3f& fcl_r){
	fcl_r.getW () = ompl_r.w;
	fcl_r.getX () = ompl_r.x;
	fcl_r.getY () = ompl_r.y;
	fcl_r.getZ () = ompl_r.z;
}
double flyingcamera::AngleChangeCosValue(const fcl::Quaternion3f& r1, const fcl::Quaternion3f& r2){
	fcl::Matrix3f m1, m2;
	r1.toRotation(m1);
	r2.toRotation(m2);
	return 1-m1.getColumn(2).dot(m2.getColumn(2));

}
